package hnsw

import (
	"compress/gzip"
	"encoding/binary"

	// "encoding/json"
	"fmt"
	"io"
	"math"
	"math/rand"
	"os"
	"sort"
	"strconv"
	"strings"
	"sync"
	"time"

	"github.com/tendermint/tendermint/my_app/querylayer/vindex/go-hnsw/bitsetpool"
	"github.com/tendermint/tendermint/my_app/querylayer/vindex/go-hnsw/distqueue"
	"github.com/tendermint/tendermint/my_app/querylayer/vindex/go-hnsw/f32"
)

type Point []float32

func (a Point) Size() int {
	return len(a) * 4
}

type node struct {
	sync.RWMutex
	locked    bool
	p         Point
	level     int
	friends   [][]uint32
	findleaf  []*TreeNode
	findpoint *TreeNode //查找时确定哪些点的真实值需要发送
}

type Hnsw struct {
	sync.RWMutex
	M              int
	M0             int
	efConstruction int
	linkMode       int
	DelaunayType   int

	DistFunc func([]float32, []float32) float32

	nodes []node

	bitset *bitsetpool.BitsetPool

	LevelMult  float64
	maxLayer   int
	enterpoint uint32
	TruthADS   *TreeNode
	ADS        []*TreeNode
	loca_point sync.Map
	levCount   []int
}

// Load opens a index file previously written by Save(). Returnes a new index and the timestamp the file was written
func Load(filename string) (*Hnsw, int64, error) {
	f, err := os.Open(filename)
	if err != nil {
		return nil, 0, err
	}
	z, err := gzip.NewReader(f)
	if err != nil {
		return nil, 0, err
	}

	timestamp := readInt64(z)

	h := new(Hnsw)
	h.M = readInt32(z)
	h.M0 = readInt32(z)
	h.efConstruction = readInt32(z)
	h.linkMode = readInt32(z)
	h.DelaunayType = readInt32(z)
	h.LevelMult = readFloat64(z)
	h.maxLayer = readInt32(z)
	h.enterpoint = uint32(readInt32(z))

	h.DistFunc = f32.L2Squared8AVX
	h.bitset = bitsetpool.New()

	l := readInt32(z)
	h.nodes = make([]node, l)

	for i := range h.nodes {

		l := readInt32(z)
		h.nodes[i].p = make([]float32, l)

		err = binary.Read(z, binary.LittleEndian, h.nodes[i].p)
		if err != nil {
			panic(err)
		}
		h.nodes[i].level = readInt32(z)

		l = readInt32(z)
		h.nodes[i].friends = make([][]uint32, l)

		for j := range h.nodes[i].friends {
			l := readInt32(z)
			h.nodes[i].friends[j] = make([]uint32, l)
			err = binary.Read(z, binary.LittleEndian, h.nodes[i].friends[j])
			if err != nil {
				panic(err)
			}
		}

	}

	z.Close()
	f.Close()

	return h, timestamp, nil
}

// Save writes to current index to a gzipped binary data file
func (h *Hnsw) Save(filename string) error {
	f, err := os.Create(filename)
	if err != nil {
		return err
	}
	z := gzip.NewWriter(f)

	timestamp := time.Now().Unix()

	writeInt64(timestamp, z)

	writeInt32(h.M, z)
	writeInt32(h.M0, z)
	writeInt32(h.efConstruction, z)
	writeInt32(h.linkMode, z)
	writeInt32(h.DelaunayType, z)
	writeFloat64(h.LevelMult, z)
	writeInt32(h.maxLayer, z)
	writeInt32(int(h.enterpoint), z)

	l := len(h.nodes)
	writeInt32(l, z)

	if err != nil {
		return err
	}
	for _, n := range h.nodes {
		l := len(n.p)
		writeInt32(l, z)
		err = binary.Write(z, binary.LittleEndian, []float32(n.p))
		if err != nil {
			panic(err)
		}
		writeInt32(n.level, z)

		l = len(n.friends)
		writeInt32(l, z)
		for _, f := range n.friends {
			l := len(f)
			writeInt32(l, z)
			err = binary.Write(z, binary.LittleEndian, f)
			if err != nil {
				panic(err)
			}
		}
	}

	z.Close()
	f.Close()

	return nil
}

func writeInt64(v int64, w io.Writer) {
	err := binary.Write(w, binary.LittleEndian, &v)
	if err != nil {
		panic(err)
	}
}

func writeInt32(v int, w io.Writer) {
	i := int32(v)
	err := binary.Write(w, binary.LittleEndian, &i)
	if err != nil {
		panic(err)
	}
}

func readInt32(r io.Reader) int {
	var i int32
	err := binary.Read(r, binary.LittleEndian, &i)
	if err != nil {
		panic(err)
	}
	return int(i)
}

func writeFloat64(v float64, w io.Writer) {
	err := binary.Write(w, binary.LittleEndian, &v)
	if err != nil {
		panic(err)
	}
}

func readInt64(r io.Reader) (v int64) {
	err := binary.Read(r, binary.LittleEndian, &v)
	if err != nil {
		panic(err)
	}
	return
}

func readFloat64(r io.Reader) (v float64) {
	err := binary.Read(r, binary.LittleEndian, &v)
	if err != nil {
		panic(err)
	}
	return
}

func (h *Hnsw) getFriends(n uint32, level int) []uint32 {
	if len(h.nodes[n].friends) < level+1 {
		return make([]uint32, 0)
	}
	return h.nodes[n].friends[level]
}

func (h *Hnsw) Link(first, second uint32, level int, use_vo bool) {

	maxL := h.M
	if level == 0 {
		maxL = h.M0
	}

	h.RLock()
	node := &h.nodes[first]
	h.RUnlock()

	node.Lock()

	// check if we have allocated friends slices up to this level?
	if len(node.friends) < level+1 {
		for j := len(node.friends); j <= level; j++ {
			// allocate new list with 0 elements but capacity maxL
			node.friends = append(node.friends, make([]uint32, 0, maxL))
		}
		// now grow it by one and add the first connection for this layer
		node.friends[level] = node.friends[level][0:1]
		node.friends[level][0] = second

	} else {
		// we did have some already... this will allocate more space if it overflows maxL
		node.friends[level] = append(node.friends[level], second)
	}

	l := len(node.friends[level])

	if l > maxL {

		// to many links, deal with it

		switch h.DelaunayType {
		case 0:
			resultSet := &distqueue.DistQueueClosestLast{Size: len(node.friends[level])}

			for _, n := range node.friends[level] {
				resultSet.Push(n, h.DistFunc(node.p, h.nodes[n].p))
			}
			for resultSet.Len() > maxL {
				resultSet.Pop()
			}
			// FRIENDS ARE STORED IN DISTANCE ORDER, closest at index 0
			node.friends[level] = node.friends[level][0:maxL]
			for i := maxL - 1; i >= 0; i-- {
				item := resultSet.Pop()
				node.friends[level][i] = item.ID
			}

		case 1:

			resultSet := &distqueue.DistQueueClosestFirst{Size: len(node.friends[level])}

			for _, n := range node.friends[level] {
				resultSet.Push(n, h.DistFunc(node.p, h.nodes[n].p))
			}
			h.getNeighborsByHeuristicClosestFirst(resultSet, maxL)

			// FRIENDS ARE STORED IN DISTANCE ORDER, closest at index 0
			node.friends[level] = node.friends[level][0:maxL]
			for i := 0; i < maxL; i++ {
				item := resultSet.Pop()
				node.friends[level][i] = item.ID
			}
		}
	}

	node.Unlock()
	//重新自下而上计算hash
	h.Lock()
	if use_vo {
		// fmt.Printf("%d ", second)
		h.MHT_Update(first, level)
	}
	h.Unlock()
}

func (h *Hnsw) getNeighborsByHeuristicClosestLast(resultSet1 *distqueue.DistQueueClosestLast, M int) {
	if resultSet1.Len() <= M {
		return
	}
	resultSet := &distqueue.DistQueueClosestFirst{Size: resultSet1.Len()}
	tempList := &distqueue.DistQueueClosestFirst{Size: resultSet1.Len()}
	result := make([]*distqueue.Item, 0, M)
	for resultSet1.Len() > 0 {
		resultSet.PushItem(resultSet1.Pop())
	}
	for resultSet.Len() > 0 {
		if len(result) >= M {
			break
		}
		e := resultSet.Pop()
		good := true
		for _, r := range result {
			if h.DistFunc(h.nodes[r.ID].p, h.nodes[e.ID].p) < e.D {
				good = false
				break
			}
		}
		if good {
			result = append(result, e)
		} else {
			tempList.PushItem(e)
		}
	}
	for len(result) < M && tempList.Len() > 0 {
		result = append(result, tempList.Pop())
	}
	for _, item := range result {
		resultSet1.PushItem(item)
	}
}

func (h *Hnsw) getNeighborsByHeuristicClosestFirst(resultSet *distqueue.DistQueueClosestFirst, M int) {
	if resultSet.Len() <= M {
		return
	}
	tempList := &distqueue.DistQueueClosestFirst{Size: resultSet.Len()}
	result := make([]*distqueue.Item, 0, M)
	for resultSet.Len() > 0 {
		if len(result) >= M {
			break
		}
		e := resultSet.Pop()
		good := true
		for _, r := range result {
			if h.DistFunc(h.nodes[r.ID].p, h.nodes[e.ID].p) < e.D {
				good = false
				break
			}
		}
		if good {
			result = append(result, e)
		} else {
			tempList.PushItem(e)
		}
	}
	for len(result) < M && tempList.Len() > 0 {
		result = append(result, tempList.Pop())
	}
	resultSet.Reset()

	for _, item := range result {
		resultSet.PushItem(item)
	}
}

func New(M int, efConstruction int, first Point) *Hnsw {

	h := Hnsw{}
	h.M = M
	// default values used in c++ implementation
	h.LevelMult = 1 / math.Log(float64(M))
	h.efConstruction = efConstruction
	h.M0 = 2 * M
	h.DelaunayType = 1

	h.bitset = bitsetpool.New()

	h.DistFunc = f32.L2Squared8AVX
	h.ADS = make([]*TreeNode, 0)
	h.TruthADS = new(TreeNode)
	// add first point, it will be our enterpoint (index 0)
	h.nodes = []node{node{level: 0, p: first, findleaf: make([]*TreeNode, 1), findpoint: new(TreeNode)}}
	h.levCount = make([]int, 1)
	h.levCount[0] = 1
	return &h
}

func (h *Hnsw) Stats() string {
	s := "HNSW Index\n"
	s = s + fmt.Sprintf("M: %v, efConstruction: %v\n", h.M, h.efConstruction)
	s = s + fmt.Sprintf("DelaunayType: %v\n", h.DelaunayType)
	s = s + fmt.Sprintf("Number of nodes: %v\n", len(h.nodes))
	s = s + fmt.Sprintf("Max layer: %v\n", h.maxLayer)
	memoryUseData := 0
	memoryUseIndex := 0
	levCount := make([]int, h.maxLayer+1)
	conns := make([]int, h.maxLayer+1)
	connsC := make([]int, h.maxLayer+1)
	for i := range h.nodes {
		levCount[h.nodes[i].level]++
		for j := 0; j <= h.nodes[i].level; j++ {
			if len(h.nodes[i].friends) > j {
				l := len(h.nodes[i].friends[j])
				conns[j] += l
				connsC[j]++
			}
		}
		memoryUseData += h.nodes[i].p.Size()
		memoryUseIndex += h.nodes[i].level*h.M*4 + h.M0*4
	}
	for i := range levCount {
		avg := conns[i] / max(1, connsC[i])
		s = s + fmt.Sprintf("Level %v: %v nodes, average number of connections %v\n", i, levCount[i], avg)
	}
	s = s + fmt.Sprintf("Memory use for data: %v (%v bytes / point)\n", memoryUseData, memoryUseData/len(h.nodes))
	s = s + fmt.Sprintf("Memory use for index: %v (avg %v bytes / point)\n", memoryUseIndex, memoryUseIndex/len(h.nodes))
	return s
}

func (h *Hnsw) Grow(size int) {
	if size+1 <= len(h.nodes) {
		return
	}
	newNodes := make([]node, len(h.nodes), size+1)
	copy(newNodes, h.nodes)
	h.nodes = newNodes
}

func (h *Hnsw) Add(q Point, id uint32, use_vo bool) {

	if id == 0 {
		panic("Id 0 is reserved, use ID:s starting from 1 when building index")
	}

	// generate random level
	curlevel := int(math.Floor(-math.Log(rand.Float64() * h.LevelMult)))

	epID := h.enterpoint
	currentMaxLayer := h.nodes[epID].level
	ep := &distqueue.Item{ID: h.enterpoint, D: h.DistFunc(h.nodes[h.enterpoint].p, q)}

	// assume Grow has been called in advance
	newID := id
	newNode := node{p: q, level: curlevel, friends: make([][]uint32, min(curlevel, currentMaxLayer)+1), findleaf: make([]*TreeNode, curlevel+1), findpoint: new(TreeNode)} //ly

	// first pass, find another ep if curlevel < maxLayer
	for level := currentMaxLayer; level > curlevel; level-- {
		changed := true
		for changed {
			changed = false
			nowpoint := ep
			for _, i := range h.getFriends(nowpoint.ID, level) {
				d := h.DistFunc(h.nodes[i].p, q)
				if d < nowpoint.D {
					ep = &distqueue.Item{ID: i, D: d}
					changed = true
				}
			}
		}
	}

	// second pass, ef = efConstruction
	// loop through every level from the new nodes level down to level 0
	// create new connections in every layer
	for level := min(curlevel, currentMaxLayer); level >= 0; level-- {

		resultSet := &distqueue.DistQueueClosestLast{}
		h.searchAtLayer(q, resultSet, h.efConstruction, ep, level, false, new(TreeNode), nil, nil, nil)
		switch h.DelaunayType {
		case 0:
			// shrink resultSet to M closest elements (the simple heuristic)
			for resultSet.Len() > h.M {
				resultSet.Pop()
			}
		case 1:
			h.getNeighborsByHeuristicClosestLast(resultSet, h.M)
		}
		newNode.friends[level] = make([]uint32, resultSet.Len())
		for i := resultSet.Len() - 1; i >= 0; i-- {
			item := resultSet.Pop()
			// store in order, closest at index 0
			newNode.friends[level][i] = item.ID
		}
	}

	h.Lock()
	// Add it and increase slice length if neccessary
	if len(h.nodes) < int(newID)+1 {
		h.nodes = h.nodes[0 : newID+1]
	}
	h.nodes[newID] = newNode
	if use_vo {
		//插入新的节点
		h.UpdateADS(newID)
	}
	if curlevel > h.maxLayer {
		h.maxLayer = curlevel
		h.enterpoint = newID
	}
	if h.maxLayer >= len(h.levCount) {
		newCount := make([]int, h.maxLayer+1)
		copy(newCount, h.levCount)
		// newCount[curlevel]++
		h.levCount = newCount
	}
	for l := 0; l <= curlevel; l++ {
		h.levCount[l]++
	}
	h.Unlock()

	// now add connections to newNode from newNodes neighbours (makes it visible in the graph)
	for level := min(curlevel, currentMaxLayer); level >= 0; level-- {
		for _, n := range newNode.friends[level] {
			h.Link(n, newID, level, use_vo)
			// h.MHT_Update(n, level)
		}
	}

	// h.Lock()
	// if curlevel > h.maxLayer {
	// 	h.maxLayer = curlevel
	// 	h.enterpoint = newID
	// }
	// h.Unlock()
}

func (h *Hnsw) searchAtLayer(q Point, resultSet *distqueue.DistQueueClosestLast, efConstruction int, ep *distqueue.Item, level int, use_vo bool, truth_root *TreeNode, bound_info *map[uint32]float32, point_info *map[int][]int8, loca_info *map[uint32]string) (map[int]Point, map[int]Point, *TreeNode, float64) {

	var pool, visited = h.bitset.Get()
	//visited := make(map[uint32]bool)

	candidates := &distqueue.DistQueueClosestFirst{Size: efConstruction * 3}

	visited.Set(uint(ep.ID))
	//visited[ep.ID] = true
	candidates.Push(ep.ID, ep.D)

	resultSet.Push(ep.ID, ep.D)

	// 可验证变量
	sorttime := 0.0
	var result_fake ItemQueue
	result_fake.New()
	result_fake.Enqueue(point{int(ep.ID), ep.D})

	// var VO_queue ItemQueue
	// VO_queue.New()
	root := new(TreeNode)
	// point_info := make(map[int]([]int8))
	// bound_info := make(map[int]float32)
	grid_info := make(map[int]Point)
	true_point_info := make(map[int]Point)
	var minID uint32
	var minD float32
	var cand []Point
	var sum int
	for candidates.Len() > 0 {
		sum++
		_, lowerBound := resultSet.Top() // worst distance so far
		c := candidates.Pop()

		if c.D > lowerBound {
			// since candidates is sorted, it wont get any better...
			break
		}
		//标记相对位置的MHT
		if use_vo {
			start := time.Now()
			//对比result_fake和resultSet,将不正确的真实数据放入truth中
			(&result_fake).Alter(int(c.ID), h.DistFunc(q, h.nodes[c.ID].p))
			sort.Sort(result_fake)
			for i := 0; i < result_fake.Len(); i++ {
				nowid := result_fake.items[i].(point).node
				// (*bound_info)[uint32(nowid)] = float32(math.Sqrt(float64(f32.L2Squared8AVX(h.nodes[uint32(nowid)].p,q))))
				if _, ok := true_point_info[nowid]; !ok {
					true_point_info[nowid] = h.nodes[uint32(nowid)].p
					leaf := h.nodes[nowid].findpoint
					truth_root = h.nodes[nowid].findpoint
					for leaf != nil {
						//标记含有结果的中间节点
						// fmt.Println(666)
						leaf.flag = true
						if truth_root.parent != nil {
							truth_root = truth_root.parent
						}
						leaf = leaf.parent
					}
				}
				if uint32(nowid) == c.ID {
					break
				} //!!!
			}
			//记录MHT
			leaf := h.nodes[c.ID].findleaf[level]
			root = h.nodes[c.ID].findleaf[level]
			grid_info[int(c.ID)] = leaf.gridsize
			for leaf != nil {
				//标记含有结果的中间节点
				leaf.flag = true
				if root.parent != nil {
					root = root.parent
				}
				leaf = leaf.parent
			}
			cand = make([]Point, 0)
			sorttime += float64(time.Since(start).Seconds())

		}

		if len(h.nodes[c.ID].friends) >= level+1 {
			friends := h.nodes[c.ID].friends[level]
			for _, n := range friends {
				if use_vo {
					start := time.Now()
					cand = append(cand, h.nodes[n].p)
					minD = c.D
					minID = c.ID
					sorttime += float64(time.Since(start).Seconds())
				}
				if !visited.Test(uint(n)) {
					visited.Set(uint(n))
					d := h.DistFunc(q, h.nodes[n].p)
					topID, topD := resultSet.Top()
					if resultSet.Len() < efConstruction {
						item := resultSet.Push(n, d)
						candidates.PushItem(item)
						minD = d
						minID = n
						// } else if topD > d {
					} else if topD > d || (topD == d && n < topID) {
						// keep length of resultSet to max efConstruction
						item := resultSet.PopAndPush(n, d)
						candidates.PushItem(item)
						minD = d
						minID = n
					}
				}
			}
		}
		if use_vo {
			start := time.Now()
			friends := h.getFriends(c.ID, level)
			grids, _, bounds := truthID(cand, h.nodes[c.ID].p, q, minD, minID, friends)
			// fmt.Println(len(grids),len(truth))
			for i, bound := range bounds {
				if _, ok := (*bound_info)[friends[i]]; ok {
					//存在则选择下界更大的
					if bound > (*bound_info)[friends[i]] {
						(*bound_info)[friends[i]] = bound
						(*point_info)[int(friends[i])] = grids[i]
						(*loca_info)[friends[i]] = "0-" + strconv.FormatUint(uint64(c.ID), 10) + "-" + strconv.FormatUint(uint64(friends[i]), 10)
					}
				} else {
					(*bound_info)[friends[i]] = bound
					(*point_info)[int(friends[i])] = grids[i]
					(*loca_info)[friends[i]] = "0-" + strconv.FormatUint(uint64(c.ID), 10) + "-" + strconv.FormatUint(uint64(friends[i]), 10)
				}
			}
			for _, i := range h.getFriends(c.ID, level) {
				dist := (*bound_info)[i]
				//把下界放入result_fake
				(&result_fake).Alter(int(i), dist*dist)
			}
			sorttime += float64(time.Since(start).Seconds())
		}
	}
	if use_vo {
		start := time.Now()
		//对比result_fake和resultSet,将不正确的真实数据放入truth中
		lastid, _ := resultSet.Top()
		// fmt.Println(resultSet.Top())
		sort.Sort(result_fake)
		for i := 0; i < result_fake.Len(); i++ {
			// fmt.Println(result_fake.items[i].(point))
			nowid := result_fake.items[i].(point).node
			if uint32(nowid) == lastid {
				break
			}
			if _, ok := true_point_info[nowid]; !ok {
				true_point_info[nowid] = h.nodes[uint32(nowid)].p
				leaf := h.nodes[nowid].findpoint
				truth_root = h.nodes[nowid].findpoint
				for leaf != nil {
					//标记含有结果的中间节点
					// fmt.Println(666)
					leaf.flag = true
					if truth_root.parent != nil {
						truth_root = truth_root.parent
					}
					leaf = leaf.parent
				}
			}
		}
		sorttime += float64(time.Since(start).Seconds())
	}
	h.bitset.Free(pool)

	return true_point_info, grid_info, root, sorttime
}

// SearchBrute returns the true K nearest neigbours to search point q
func (h *Hnsw) SearchBrute(q Point, K int) *distqueue.DistQueueClosestLast {
	resultSet := &distqueue.DistQueueClosestLast{Size: K}
	for i := 1; i < len(h.nodes); i++ {
		d := h.DistFunc(h.nodes[i].p, q)
		if resultSet.Len() < K {
			resultSet.Push(uint32(i), d)
			continue
		}
		_, topD := resultSet.Head()
		if d < topD {
			resultSet.PopAndPush(uint32(i), d)
			continue
		}
	}
	return resultSet
}

// Benchmark test precision by comparing the results of SearchBrute and Search
func (h *Hnsw) Benchmark(q Point, ef int, K int) float64 {
	result, _, _, _, _, _, _, _ := h.Search(q, ef, K, nil)
	groundTruth := h.SearchBrute(q, K)
	truth := make([]uint32, 0)
	for groundTruth.Len() > 0 {
		truth = append(truth, groundTruth.Pop().ID)
	}
	p := 0
	for result.Len() > 0 {
		i := result.Pop()
		for j := 0; j < K; j++ {
			if truth[j] == i.ID {
				p++
			}
		}
	}
	return float64(p) / float64(K)
}

func (h *Hnsw) Search(q Point, ef int, K int, ADS []*TreeNode) (*distqueue.DistQueueClosestLast, int, []string, string, map[int]([]int8), map[int]Point, []map[int]Point, float64) {

	h.RLock()
	currentMaxLayer := h.maxLayer
	ep := &distqueue.Item{ID: h.enterpoint, D: h.DistFunc(h.nodes[h.enterpoint].p, q)}
	h.RUnlock()

	resultSet := &distqueue.DistQueueClosestLast{Size: ef + 1}

	use_vo := (len(ADS) != 0)
	// var VO_queue ItemQueue
	// VO_queue.New()
	vo_temp := make([]string, currentMaxLayer+1)
	enterpoint := int(ep.ID)
	sorttime := 0.0

	//相对位置索引表
	point_info := make(map[int]([]int8))
	//网格大小索引表
	grid_info := make([]map[int]Point, currentMaxLayer+1)
	for i := 0; i < currentMaxLayer+1; i++ {
		// point_info[i] = make(map[string][]int8)
		grid_info[i] = make(map[int]Point)
	}
	//真实位置索引表
	true_point_info := make(map[int]Point)
	//距离下界索引表
	bound_info := make(map[uint32]float32)
	//记录层数的索引表
	loca_info := make(map[uint32]string)

	truth_root := new(TreeNode)
	final_root := make([]*TreeNode, currentMaxLayer+1)
	// first pass, find best ep
	for level := currentMaxLayer; level > 0; level-- {
		changed := true
		root := new(TreeNode)
		for changed {
			changed = false
			// fmt.Println(ep.ID)
			//标记相对位置的MHT
			if use_vo {
				start := time.Now()
				true_point_info[int(ep.ID)] = h.nodes[ep.ID].p

				leaf := h.nodes[ep.ID].findleaf[level]
				root = h.nodes[ep.ID].findleaf[level]
				grid_info[level][int(ep.ID)] = leaf.gridsize
				for leaf != nil {
					//标记含有结果的中间节点
					// fmt.Println(666)
					leaf.flag = true
					if root.parent != nil {
						root = root.parent
					}
					leaf = leaf.parent
				}

				sorttime += float64(time.Since(start).Seconds())
			}

			nowid := ep.ID
			var candidates []Point
			for _, i := range h.getFriends(nowid, level) {
				d := h.DistFunc(h.nodes[i].p, q)
				// fmt.Print(i, (math.Sqrt(float64(d))),",")
				if use_vo {
					candidates = append(candidates, h.nodes[i].p)
				}
				// if d < ep.D {
				// 	ep.ID, ep.D = i, d
				// 	changed = true
				// }
				if d < ep.D || (d == ep.D && i < ep.ID) {
					ep.ID, ep.D = i, d
					changed = true
				}
			}
			// fmt.Println(ep.ID, (math.Sqrt(float64(ep.D))))
			if use_vo {
				start := time.Now()
				//计算相对位置和需要发送真实位置的点
				friends := h.getFriends(nowid, level)
				grids, truth, bounds := truthID(candidates, h.nodes[nowid].p, q, ep.D, ep.ID, friends)
				// fmt.Println(len(grids),len(truth))
				//保存下界最大的相对位置,单id索引相对位置
				for i, bound := range bounds {
					if _, ok := bound_info[friends[i]]; ok {
						if bound > bound_info[friends[i]] {
							bound_info[friends[i]] = bound
							point_info[int(friends[i])] = grids[i]
							loca_info[friends[i]] = strconv.FormatUint(uint64(level), 10) + "-" + strconv.FormatUint(uint64(nowid), 10) + "-" + strconv.FormatUint(uint64(friends[i]), 10)
						}
					} else {
						bound_info[friends[i]] = bound
						point_info[int(friends[i])] = grids[i]
						loca_info[friends[i]] = strconv.FormatUint(uint64(level), 10) + "-" + strconv.FormatUint(uint64(nowid), 10) + "-" + strconv.FormatUint(uint64(friends[i]), 10)
					}
				}
				//单id索引相对位置

				//单id索引真实位置
				truth = append(truth, nowid)
				for _, id := range truth {
					if _, ok := true_point_info[int(id)]; !ok {
						true_point_info[int(id)] = h.nodes[id].p
					}
					// bound_info[id] = float32(math.Sqrt(float64(f32.L2Squared8AVX(h.nodes[id].p,q))))
					leaf := h.nodes[id].findpoint
					truth_root = h.nodes[id].findpoint
					for leaf != nil {
						//标记含有结果的中间节点
						// fmt.Println(666)
						leaf.flag = true
						if truth_root.parent != nil {
							truth_root = truth_root.parent
						}
						leaf = leaf.parent
					}
				}
				sorttime += float64(time.Since(start).Seconds())
			}
		}
		final_root[level] = root
	}
	knn_true_point_info, knn_grid_info, knn_root, stime := h.searchAtLayer(q, resultSet, ef, ep, 0, true, truth_root, &bound_info, &point_info, &loca_info)

	// point_info[0] = knn_point_info
	if use_vo {
		start := time.Now()
		grid_info[0] = knn_grid_info
		final_root[0] = knn_root
		for k, v := range knn_true_point_info {
			if _, ok := true_point_info[k]; !ok {
				true_point_info[k] = v
			}
		}

		for k := range true_point_info {
			if _, ok := point_info[k]; ok {
				// fmt.Println("666")
				delete(point_info, k)
				delete(loca_info, uint32(k))
			}
		}
		// fmt.Println(len(true_point_info))

		//去掉多余的网格大小信息
		for level := currentMaxLayer; level >= 0; level-- {
			level_s := strconv.Itoa(level)
			for center := range grid_info[level] {

				center_s := strconv.Itoa(int(center))
				reserve := false
				for _, v := range loca_info {
					strArr := strings.Split(v, "-")
					if level_s == strArr[0] && center_s == strArr[1] {
						// fmt.Println(strArr)
						reserve = true
						break
					}
				}
				if !reserve {
					delete(grid_info[level], center)
				}
			}
		}
		// fmt.Println(len(point_info))
		//根据位置构造subMHT的VO
		// loca_point.Range(func(key, value interface{}) bool {
		// 	fmt.Printf("键：%v", key )
		// 	return true // 返回true以继续迭代，或者返回false以停止迭代
		// })
		for _, v := range loca_info {
			// fmt.Println(v)
			value, _ := h.loca_point.Load(v)
			leaf := value.(*TreeNode)
			root := value.(*TreeNode)
			for leaf != nil {
				//标记含有结果的中间节点
				leaf.flag = true
				if root.parent != nil {
					root = root.parent
				}
				leaf = leaf.parent
			}
		}

		var wg sync.WaitGroup
		for level := currentMaxLayer; level >= 0; level-- {
			wg.Add(1)
			go func(level int) {
				defer wg.Done()
				vo := h.getVO(final_root[level], level)
				vo_temp[level] = vo
			}(level)
			// fmt.Println(vo)
		}
		wg.Wait()
		// for level := currentMaxLayer; level >= 0; level-- {
		// 	VO_queue.Enqueue(VO{value:vo_temp[level]})
		// }

		sorttime += float64(time.Since(start).Seconds())
	}
	//构建真实位置的MHT的VO
	truth_VO := h.gettruthVO(truth_root)

	sorttime += stime

	for resultSet.Len() > K {
		resultSet.Pop()
	}
	return resultSet, enterpoint, vo_temp, truth_VO, point_info, true_point_info, grid_info, sorttime
}

func min(a, b int) int {
	if a < b {
		return a
	}
	return b
}

func max(a, b int) int {
	if a > b {
		return a
	}
	return b
}
